{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import symbols, cos, sin, pi, simplify, pprint, expand_trig\n",
    "from sympy.matrices import Matrix\n",
    "\n",
    "\n",
    "def rotx(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [1., 0., 0.],\n",
    "    [0., cq,-sq],\n",
    "    [0., sq, cq]\n",
    "  ])\n",
    "    \n",
    "  return r\n",
    "\n",
    "\n",
    "def roty(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [ cq, 0., sq],\n",
    "    [ 0., 1., 0.],\n",
    "    [-sq, 0., cq]\n",
    "  ])\n",
    "    \n",
    "  return r\n",
    "\n",
    "\n",
    "def rotz(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [cq,-sq, 0.],\n",
    "    [sq, cq, 0.],\n",
    "    [0., 0., 1.]\n",
    "  ])\n",
    "    \n",
    "  return r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡ 0    0    1⎤\n",
      "⎢            ⎥\n",
      "⎢ 0   -1.0  0⎥\n",
      "⎢            ⎥\n",
      "⎣1.0   0    0⎦\n",
      "⎡0   0    1.0⎤\n",
      "⎢            ⎥\n",
      "⎢0  -1.0   0 ⎥\n",
      "⎢            ⎥\n",
      "⎣1   0     0 ⎦\n",
      "True\n",
      "True\n"
     ]
    }
   ],
   "source": [
    "# the yaw, pitch roll is given wrt to the URDF frame \n",
    "# We must convert this to gripper frame by performing\n",
    "# a rotation of 180 degrees ccw about the z axis and then \n",
    "# a rotation of 90 degrees cw about the new y axis\n",
    "\n",
    "r = rotz(pi) * roty(-pi/2) \n",
    "\n",
    "pprint(r)\n",
    "pprint(r.T)\n",
    "\n",
    "result1 = r * Matrix([0,0,1])\n",
    "result2 = r.T * Matrix([0,0,1])\n",
    "print(result1 == result2)\n",
    "print(r.T == r)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "RguT = Matrix([[0,0,1],[0, -1, 0],[1,0,0]]) #RguT = (rotz(pi) * roty(-pi/2)).T\n",
    "\n",
    "def get_wrist_center(xu, yu, zu, R0u, dg = 0.303):\n",
    "  # get the coordinates of the wrist center wrt to the base frame (xw, yw, zw)\n",
    "  # given the following info:\n",
    "  # the coordinates of the gripper (end effector) (x, y, z)\n",
    "  # the rotation of the gripper in URDF frame wrt to the base frame (R0u)\n",
    "  # the distance between gripper and wrist center dg\n",
    "  # which is along common z axis\n",
    "    \n",
    "  nx, ny, nz = R0u[0, 2], R0u[1, 2], R0u[2, 2]\n",
    "  xw = xu - dg * nx\n",
    "  yw = yu - dg * ny\n",
    "  zw = zu - dg * nz \n",
    "\n",
    "  return xw, yw, zw"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "wrist center:\n",
      "(0.750499428337951, 1.20160389781975, 2.47518995758694)\n"
     ]
    }
   ],
   "source": [
    "px, py, pz = 0.49792, 1.3673, 2.4988\n",
    "roll, pitch, yaw = 0.366, -0.078, 2.561\n",
    "\n",
    "R0u = rotz(yaw) * roty(pitch) * rotx(roll) * RguT\n",
    "\n",
    "print(\"wrist center:\")\n",
    "print(get_wrist_center(px, py, pz, R0u, dg = 0.303))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "wrist center:\n",
      "(1.85000000000000, 0, 1.94600000000000)\n"
     ]
    }
   ],
   "source": [
    "px, py, pz = 2.153, 0, 1.946\n",
    "roll, pitch, yaw = 0, 0, 0\n",
    "\n",
    "R0u = rotz(yaw) * roty(pitch) * rotx(roll) * RguT\n",
    "\n",
    "print(\"wrist center:\")\n",
    "print(get_wrist_center(px, py, pz, R0u, dg = 0.303))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
